#include <caros/kuka_robots.h>

#include <caros/common.h>
#include <caros/common_robwork.h>

#include <rw/invkin/JacobianIKSolver.hpp>
#include <rw/math/MetricFactory.hpp>

#include <ros/assert.h>

#include <vector>
#include <string>

namespace caros
{
void update_robot_state(std::string input, void* v_state)
{
  //     std::cout << "calling the right function" << std::endl;
  //     std::cerr << "[inc]: '" << input << "'" <<std::endl;
  kuka_lwr* r_state = reinterpret_cast<kuka_lwr*>(v_state);

  std::vector<std::string> variables;
  tokenize(input, variables, ':');

  // "get tool frame"      response gives "tcp : 6 doubles"
  // "get flange frame"    response gives "tcp : 6 doubles"
  // "get status"          response gives "stopped|moving"
  // "get cartesian force" response gives "cartesian ft : 6 doubles"
  // "get joint position"  response gives "joint conf : 7 doubles"
  // "get joint torque"    response gives "joint torque : 7 doubles"
  // garbage               response gives "unknown command"
  // "hello"               response gives "hello"
  // everything else       response gives "error|done"
  for (size_t i = 0; i < variables.size(); ++i)
  {
    if (variables.at(i) == "tcp ")
    {
      r_state->set_pos(variables, i);
      i += 6;
    }
    else if (variables.at(i) == "cartesian ft ")
    {
      r_state->set_cart_force(variables, i);
      i += 6;
    }
    else if (variables.at(i) == "joint conf ")
    {
      r_state->set_q(variables, i);
      i += 7;
    }
    else if (variables.at(i) == "joint torque ")
    {
      r_state->set_force_q(variables, i);
      i += 7;
    }
    else if (variables.at(i) == "stopped")
    {
      r_state->set_moving(false);
    }
    else if (variables.at(i) == "moving")
    {
      r_state->set_moving(true);
    }
    else if (variables.at(i) == "error")
    {
      r_state->success_ = false;
    }
    else if (variables.at(i) == "done")
    {
      r_state->success_ = true;
    }
    else if (variables.at(i) == "hello")
    {
      //             std::cout << "hello\n";
    }
  }
  //     r_state->new_message_ = true;

  r_state->publish();
}

KukaRobots::KukaRobots(const ros::NodeHandle& nodehandle)
    : CarosNodeServiceInterface(nodehandle),
      SerialDeviceServiceInterface(nodehandle),
      nodehandle_(nodehandle),
      qcurrent_(7),
      current_robot_state_(NULL),
      io_service_(),
      tcp_thread_(),
      client_sock_(NULL)
{
  /* Currently nothing specific should happen */
}

KukaRobots::~KukaRobots()
{
  std::cerr << "destroying kukarobots" << std::endl;
  client_sock_->close();
  tcp_thread_.join();
}

bool KukaRobots::activateHook()
{
  /* TODO:
   * - Maybe split all this configuration/activation logic up into a configure and connect step, like what has been done
   * for the SDH node.
   * ^- Should the fetching of the parameters be moved into its own function, instead of having this big activateHook()
   * function?
   */
  /************************************************************************
   * Parameters
   ************************************************************************/

  std::string ft_topic;
  if (!nodehandle_.getParam("ft_topic", ft_topic))
  {
    CAROS_FATALERROR("The parameter '" << nodehandle_.getNamespace()
                                       << "/ft_topic' was not present on the "
                                          "parameter server! This parameter has to be "
                                          "specified for this node to work properly.",
                     KUKANODE_MISSING_PARAMETER);
    return false;
  }
  current_robot_state_ = rw::common::ownedPtr(new kuka_lwr(ft_topic));
  std::string device_ip;
  if (!nodehandle_.getParam("device_ip", device_ip))
  {
    CAROS_FATALERROR("The parameter '" << nodehandle_.getNamespace()
                                       << "/device_ip' was not present on the parameter server! "
                                          "This parameter has to be specified for this "
                                          "node to work properly.",
                     KUKANODE_MISSING_PARAMETER);
    return false;
  }

  std::string device_port;
  if (!nodehandle_.getParam("device_port", device_port))
  {
    CAROS_FATALERROR("The parameter '" << nodehandle_.getNamespace()
                                       << "/device_port' was not present on the "
                                          "parameter server! This parameter has to be "
                                          "specified for this node to work properly.",
                     KUKANODE_MISSING_PARAMETER);
    return false;
  }
  boost::asio::ip::tcp::resolver resolver(io_service_);
  boost::asio::ip::tcp::resolver::iterator endpoint_iterator = resolver.resolve(
                                                                                {device_ip, device_port}
                                                                                /*end of scope */);
  client_sock_ = rw::common::ownedPtr(
      new tcp_client(io_service_, endpoint_iterator, update_robot_state, current_robot_state_.get()));
  std::cerr << "client socket is created" << std::endl;

  tcp_thread_ = std::thread([this]()
                            {
                              io_service_.run();
                            }
                            /*end of code*/);
  set_robot_feedbacks(LWR_JOINT_POS | /*LWR_TOOL_FRAME |*/ LWR_STATUS | /*LWR_JOINT_TORQUE |*/ LWR_CART_FORCE);

  srv_raw_message_ = nodehandle_.advertiseService("kuka_raw_message", &KukaRobots::KukaWriteRaw, this);

  SerialDeviceServiceInterface::configureInterface();
  return true;
}

bool KukaRobots::recoverHook(const std::string& error_msg, const int64_t error_code)
{
  /* TODO: Missing handling all the different scenarios. */
  /* TODO: Go into fatal error if recover is invoked on the unsupported error scenarios */
  bool resolved = false;

  switch (error_code)
  {
    case KUKANODE_UNSUPPORTED_Q_LENGTH:
      /* Simply acknowledge that a wrong Q was provided */
      resolved = true;
      break;
    default:
      CAROS_FATALERROR("The provided error code '"
                           << error_code << "' has no recovery functionality! - this should be considered a bug!",
                       KUKANODE_INTERNAL_ERROR);
      break;
  }

  return resolved;
}

void KukaRobots::runLoopHook()
{
  if (current_robot_state_ == NULL)
    return;
  if (!current_robot_state_->new_message_)
  {  // if set to false, another function is waiting for a message and there is no need to send additional messages
    client_sock_->write("hello");
  }
  if (!client_sock_->running())
  {
    CAROS_FATALERROR("The communication to the java server has shut down.", KUKANODE_INTERNAL_ERROR);
  }

  caros_control_msgs::RobotState robot_state = current_robot_state_->to_caros();
  qcurrent_ = caros::toRw(robot_state.q);
  robot_state.header.frame_id = nodehandle_.getNamespace();

  //   std::cerr << "ran loop" << std::endl;
  //   SerialDeviceServiceInterface::publishState(robot_state);
  //   current_robot_state_->publish(); //publish the FT data as wrench
}

void KukaRobots::errorLoopHook()
{
  /* TODO:
   * Consider what needs to be done when this node is in error
   */
}

void KukaRobots::fatalErrorLoopHook()
{
  ros::requestShutdown();
  /* TODO:
   * Consider what needs to be done when this node is in error
   */
}

void KukaRobots::write(std::string msg)
{
  current_robot_state_->new_message_ = true;
  client_sock_->write(msg);
}

bool KukaRobots::KukaWriteRaw(caros_kuka_lwr::KukaWriteRawService::Request& request,
                              caros_kuka_lwr::KukaWriteRawService::Response& response)
{
  std::cerr << "service call: " << request.msg << std::endl;
  write(request.msg);

  //   while(!current_robot_state_->new_message_)
  //       usleep(1000);
  //   response.success = current_robot_state_->success_;

  return true;
}

/************************************************************************
 * SerialDeviceServiceInterface
 ************************************************************************/
bool KukaRobots::moveLin(const TransformAndSpeedAndBlendContainer_t& targets)  // vector<T, float, float>
{
  ROS_DEBUG_STREAM("moveLin with " << targets.size() << " target(s).");

  if (!isInWorkingCondition())
    return false;

  for (const auto& target : targets)
  {
    write(lin_move(std::get<0>(target), std::get<1>(target) / 100.0 /*, std::get<2>(target)*/));
  }
  return true;
}

bool KukaRobots::movePtp(const QAndSpeedAndBlendContainer_t& targets)
{
  ROS_DEBUG_STREAM("movePtp with " << targets.size() << " target(s).");
  if (!isInWorkingCondition())
    return false;

  for (const auto& target : targets)
  {
    // TODO(nivii): add blend to the move_q/joint move function.
    rw::math::Q q = std::get<0>(target);
    rw::math::Q vel(7, std::get<1>(target) / 100.0);
    if (!supportedQSize(q))
      return false;
    write(move_q(q, vel, std::get<2>(target)));
  }

  return true;
}

bool KukaRobots::movePtpT(const TransformAndSpeedAndBlendContainer_t& targets)
{
  ROS_DEBUG_STREAM("movePtpT with " << targets.size() << " target(s).");
  if (!isInWorkingCondition())
    return false;

  for (const auto& target : targets)
  {
    write(ptp_move(std::get<0>(target), std::get<1>(target) / 100.0, std::get<2>(target)));
  }

  return true;
}

bool KukaRobots::moveVelQ(const rw::math::Q& q_vel)
{
  ROS_ERROR_STREAM("Current implementation does not follow the specification!");
  return false;
}

bool KukaRobots::moveVelT(const rw::math::VelocityScrew6D<>& t_vel)
{
  ROS_ERROR_STREAM("Current implementation has not been verified to follow the specification!");
  return false;
}

bool KukaRobots::moveServoQ(const QAndSpeedContainer_t& targets)
{
  if (!isInWorkingCondition())
    return false;

  for (const auto& target : targets)
  {
    rw::math::Q q = std::get<0>(target);
    rw::math::Q vel(7, std::get<1>(target) / 100.0);
    if (!supportedQSize(q))
      return false;
    std::string msg = move_q(q, vel);
    write(msg);
  }

  return true;
}

bool KukaRobots::moveServoT(const TransformAndSpeedContainer_t& targets)
{
  if (!isInWorkingCondition())
    return false;

  for (const auto& target : targets)
  {
    write(ptp_move(std::get<0>(target), std::get<1>(target) / 100.0));
  }

  return true;
}

bool KukaRobots::moveStop()
{
  if (!isInWorkingCondition())
    return false;

  write(stop());
  return true;
}

/************************************************************************
 * Utility functions
 ************************************************************************/
bool KukaRobots::isInWorkingCondition()
{
  if (!isInRunning())
  {
    ROS_WARN_STREAM("Not in running state!");
    return false;
  }
  return true;
}

bool KukaRobots::supportedQSize(const rw::math::Q& q)
{
  if (q.size() != SUPPORTED_Q_LENGTH_FOR_KUKA)
  {
    CAROS_ERROR("The length of Q is " << q.size() << " but should be " << SUPPORTED_Q_LENGTH_FOR_KUKA,
                KUKANODE_UNSUPPORTED_Q_LENGTH);
    return false;
  }
  return true;
}

void KukaRobots::set_robot_feedbacks(char states)
{
  client_sock_->write(std::string("feedback: frame ") +
                      ((states & LWR_TOOL_FRAME) ? "Tool" : (states & LWR_FLANGE_FRAME ? "Flange" : "None")));
  usleep(100);
  client_sock_->write(std::string("feedback: joint_position ") + ((states & LWR_JOINT_POS ? "True" : "False")));
  usleep(100);
  client_sock_->write(std::string("feedback: status ") + ((states & LWR_STATUS ? "True" : "False")));
  usleep(100);
  client_sock_->write(std::string("feedback: cartesian_force ") + ((states & LWR_CART_FORCE ? "True" : "False")));
  usleep(100);
  client_sock_->write(std::string("feedback: joint_torque ") + ((states & LWR_JOINT_TORQUE ? "True" : "False")));
  usleep(100);
}

void tokenize(const std::string& input, std::vector<std::string>& output, char token)
{
  size_t start_string = 0;
  for (size_t i = 1; i < input.size(); ++i)
  {
    if (input.at(i) == token && input.at(i - 1) != token)
    {
      output.push_back(input.substr(start_string, i - start_string));
      start_string = i + 1;
    }
    else if (i == input.size() - 1)
    {
      output.push_back(input.substr(start_string, i - start_string + 1));
    }
  }
}

}  // namespace caros
